#include <iostream>
#include "Eigen/Dense"
#include <Eigen/Core>
#include <cmath>
#include <iomanip>
#include<vector>

using namespace Eigen;
using Eigen::MatrixXd;
using namespace std;
const double pi = acos(-1.0);

class kinematics
{
private:
	double L1 = 436, L2 = 46.5, L3 = 246.75, L = 20;//L1表示为十字轴到端面的长度,L2表示3坐标系相对于2坐标系沿着X轴的距离,L3表示5坐标系相对于4坐标系z轴的距离，L4表示末端工作位置点相对于5坐标系沿着z轴方向的距离
	double a = 0, b = 0, H1, H2, x, y, z, X, Y, Z, U1, U2, e;//过渡矩阵临时变量
	double m2_up, m2_down, m2_m, error, res_up, res_down, res_m;//二分法临时变量
	double a1, a2, c;
	double m1, m2, m3, m4, m5, m6, m_dj_1, m_dj_2, m_dj_3, m_dj_4,h;
	double r, r1, r2, r3, r4, R = 14, l1, l2, l3, l4, Ldd = 12, l10 = 24 * 6, l20 = 24 * 6, l30 = 24 * 6, l40 = 24 * 6;
	double Line_1, Line_2, Line_3, Line_4, Line = 24;
	double j_m3, ja, jb;

	Matrix4d T01, T12, T23, T34, T45, T56, T67, T67N, Tp, T03, Tr, Tz, Ty, Tx, Tb, T07;
	double g1(double x)
	{
		double a = 0;
		for (int i = 1; i < 8; i++)
		{
			a += 2 * 20 * cos((x) / 2) * sin(((x) / 2) + (i - 1) * (x));
		}
		a += sin(7 * x)*46.5;
		return a;
	}
	double g2(double x) //运动学逆解中间函数
	{
		double a = 0;
		for (int i = 1; i < 8; i++)
		{
			a += 2 * 20 * cos((x) / 2) * cos(((x) / 2) + (i - 1) * (x));
		}
		a += cos(7 * x)*46.5;
		return a;
	}
	double jie(double x1, double x2, double x3, double x4, double x5) //运动学逆解中间函数
	{
		double b;
		b = x1 * x1 + (x2 + 436)*(x2 + 436) - x3 * x3 - x4 * x4 - x5 * x5;
		return b;
	}
	double jie2(double x1, double x2, double x3, double x4, double x5)
	{
		double b = 0, o1 = 0;
		j_m3 = acos(x5 / x1);
		ja = sin(j_m3)*x1; jb = x2 + 436;
		o1 = pi - asin(x3 / sqrt(ja*ja + jb * jb)) - asin(jb / (sqrt(ja*ja + jb * jb)));
		h = 238 - 52 / tan(pi / 2 - o1);
		b = x1 * x1 + (x2 + 436)*(x2 + 436) - x3 * x3 - (x4 - h) * (x4 - h) - x5 * x5;
		return b;
	}


public:

	double theta_1, theta_2, theta_3, theta_4, theta_5, theta_6;
	double theta_z, theta_y, theta_x, Px, Py, Pz;
	// double m1, m2, m3, m4, m5, m6, m_dj_1, m_dj_2, m_dj_3, m_dj_4;

	std::vector<double> Inv_sol()//逆解函数，输入三个欧拉角theta_z theta_y theta_x与三个位置Px Py Pz，输出向量前六项为m1—m6六个关节角及后四项为m_dj_1—m_dj_4四个线驱电机角度
	{
		std::vector<double> T_theta(11);

		Tz << cos(theta_z), -sin(theta_z), 0, 0,
			sin(theta_z), cos(theta_z), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		Ty << cos(theta_y), 0, sin(theta_y), 0,
			0, 1, 0, 0,
			-sin(theta_y), 0, cos(theta_y), 0,
			0, 0, 0, 1;
		Tx << 1, 0, 0, 0,
			0, cos(theta_x), -sin(theta_x), 0,
			0, sin(theta_x), cos(theta_x), 0,
			0, 0, 0, 1;
		Tb = Tz * Ty*Tx;
		// cout << Tb << endl;
		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				T07(i, j) = Tb(i, j);
			}
		}
		T07(0, 3) = Px;
		T07(1, 3) = Py;
		T07(2, 3) = Pz;
		T07(3, 3) = 1;
		T07(3, 0) = 0;
		T07(3, 1) = 0;
		T07(3, 2) = 0;

		T67 << 1, 0, 0, 0,
			0, 0, -1, 0,
			0, 1, 0, L3,
			0, 0, 0, 1;
		T67N << 1, 0, 0, 0,
			0, 0, 1, -L3,
			0, -1, 0, 0,
			0, 0, 0, 1;
		Tp = T07 * T67N;
		x = Tp(0, 3); y = Tp(1, 3); z = Tp(2, 3);
		m2_up = 1.57;//修改之前0.17
		m2_down = 0.001;
		error = 0.00000001;

		res_up = jie(g1(m2_up), g2(m2_up), x, y, z);
		res_down = jie(g1(m2_down), g2(m2_down), x, y, z);
		while (res_up*res_down < 0)
		{
			m2_m = 0.5*(m2_up + m2_down);
			res_m = jie(g1(m2_m), g2(m2_m), x, y, z);
			if (res_m*res_down < 0)
			{
				m2_up = m2_m;
			}
			else
			{
				m2_down = m2_m;
			}
			if (fabs(m2_up - m2_down) < error)
			{
				break;
			}
		}
		m2 = (m2_up + m2_down) / 2;
		m3 = acos(z / g1(m2));
		a1 = g2(m2) + L1; a2 = g1(m2)*sin(m3);
		m1 = asin((a1*y + a2 * x) / (a1 *a1 + a2 * a2));

		T01 << cos(m1), -sin(m1), 0, 0,
			sin(m1), cos(m1), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		T12 << 0, 0, 1, L1,
			0, -1, 0, 0,
			1, 0, 0, 0,
			0, 0, 0, 1;
		X = (g1(m2) - sin(7 * m2)*46.5)*cos(m3);
		Y = (g1(m2) - sin(7 * m2)*46.5)*sin(m3);
		Z = (g2(m2) - cos(7 * m2)*46.5);
		T23 << cos(m3)*cos(m3) * cos(7 * m2) + sin(m3)*sin(m3), cos(m3)*sin(m3)* cos(7 * m2) - cos(m3)*sin(m3), cos(m3)* sin(7 * m2), X,
			cos(m3)* sin(m3)* cos(7 * m2) - cos(m3) * sin(m3), sin(m3)* sin(m3)* cos(7 * m2) + cos(m3) * cos(m3), sin(m3)* sin(7 * m2), Y,
			-cos(m3) * sin(7 * m2), -sin(m3) * sin(7 * m2), cos(7 * m2), Z,
			0, 0, 0, 1;
		T03 = T01 * T12*T23;
		Tr = T03.inverse() *T07*T67N;

		m5 = acos(-Tr(1, 2));
		m4 = asin(-Tr(0, 2) / sin(m5));
		m6 = asin(-Tr(1, 1) / sin(m5));

		r = Ldd / (tan(m2 / 2));
		r1 = r - R * cos(m3);
		r2 = r - R * sin(m3);
		r3 = r + R * cos(m3);
		r4 = r + R * sin(m3);

		l1 = r1 * 2 * sin(m2 / 2) * 6;
		l2 = r2 * 2 * sin(m2 / 2) * 6;
		l3 = r3 * 2 * sin(m2 / 2) * 6;
		l4 = r4 * 2 * sin(m2 / 2) * 6;

		m_dj_1 = (l1 - l10) / 4.5;
		m_dj_2 = (l2 - l20) / 4.5;
		m_dj_3 = (l3 - l30) / 4.5;
		m_dj_4 = (l4 - l40) / 4.5;

		T_theta[0] = m1; T_theta[1] = m2; T_theta[2] = m3;
		T_theta[3] = m4; T_theta[4] = m5; T_theta[5] = m6;
		T_theta[6] = m_dj_1; T_theta[7] = m_dj_2; T_theta[8] = m_dj_3;
		T_theta[9] = m_dj_4; T_theta[10] = 0;
		return T_theta;

	}
	std::vector<double> Inv_sol_addh(int log)//逆解函数，输入三个欧拉角theta_z theta_y theta_x与三个位置Px Py Pz，输出向量前六项为m1—m6六个关节角及后四项为m_dj_1—m_dj_4四个线驱电机角度和高度h
	{
		std::vector<double> T_theta_addh(11);

		Tz << cos(theta_z), -sin(theta_z), 0, 0,
			sin(theta_z), cos(theta_z), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		Ty << cos(theta_y), 0, sin(theta_y), 0,
			0, 1, 0, 0,
			-sin(theta_y), 0, cos(theta_y), 0,
			0, 0, 0, 1;
		Tx << 1, 0, 0, 0,
			0, cos(theta_x), -sin(theta_x), 0,
			0, sin(theta_x), cos(theta_x), 0,
			0, 0, 0, 1;
		Tb = Tz * Ty*Tx;
		//cout << Tb << endl;
		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				T07(i, j) = Tb(i, j);
			}
		}
		T07(0, 3) = Px;
		T07(1, 3) = Py;
		T07(2, 3) = Pz;
		T07(3, 3) = 1;
		T07(3, 0) = 0;
		T07(3, 1) = 0;
		T07(3, 2) = 0;

		T67 << 1, 0, 0, 0,
			0, 0, -1, 0,
			0, 1, 0, L3,
			0, 0, 0, 1;
		T67N << 1, 0, 0, 0,
			0, 0, 1, -L3,
			0, -1, 0, 0,
			0, 0, 0, 1;
		Tp = T07 * T67N;
		x = Tp(0, 3); y = Tp(1, 3); z = Tp(2, 3);
		double u2;
		u2 = jie2(g1(0.1571), g2(0.1571), x, y, z);
		m2_up = 0.4;
		m2_down = 0.025;
		// m2_up = 1.57;
		// m2_down = 0.001;
		error = 0.00000001;
		//for (; z / g1(m2_down) > 1; m2_down += 0.01);
		//m2_up = 7.587 / 180 * pi;
		//cout << g1(m2_up) << "  " << g2(m2_up) << endl;
		res_up = jie2(g1(m2_up), g2(m2_up), x, y, z);
		res_down = jie2(g1(m2_down), g2(m2_down), x, y, z);

		//u2=sin(1.2217)*sin(1.3963)*g1(0.1570)+ cos(1.2217)*(g2(0.1570) + L1);
		while (res_up*res_down < 0)
		{
			m2_m = 0.5*(m2_up + m2_down);
			res_m = jie2(g1(m2_m), g2(m2_m), x, y, z);
			if (res_m*res_down < 0)
			{
				m2_up = m2_m;
			}
			else
			{
				m2_down = m2_m;
			}
			if (fabs(m2_up - m2_down) < error)
			{
				break;
			}
		}
		m2 = (m2_up + m2_down) / 2;
		m3 = acos(z / g1(m2));
		a1 = g2(m2) + L1; a2 = g1(m2)*sin(m3);
		//m1 = asin((a1*y + a2 * x) / (a1 *a1 + a2 * a2));
		m1 = pi - asin(x / sqrt(a2*a2 + a1 * a1)) - asin(a1 / (sqrt(a2*a2 + a1 * a1)));
		if(log == 2 | log == 7){
			h = 238 - 49.07 / tan(pi / 2 - m1);
		}else if(log == 3 | log == 8){
			h = 238 - 55.41 / tan(pi / 2 - m1);
		}else{
			h = 238 - 50.59 / tan(pi / 2 - m1);
		}
		//m1 = m1 * 180 / pi;
		//double u1;
		//u1 = a1 * a1 + g1(m2) * g1(m2) - x * x - y * y - z * z;

		T01 << cos(m1), -sin(m1), 0, 0,
			sin(m1), cos(m1), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		T12 << 0, 0, 1, L1,
			0, -1, 0, 0,
			1, 0, 0, 0,
			0, 0, 0, 1;
		X = (g1(m2) - sin(7 * m2)*46.5)*cos(m3);
		Y = (g1(m2) - sin(7 * m2)*46.5)*sin(m3);
		Z = (g2(m2) - cos(7 * m2)*46.5);
		T23 << cos(m3)*cos(m3) * cos(7 * m2) + sin(m3)*sin(m3), cos(m3)*sin(m3)* cos(7 * m2) - cos(m3)*sin(m3), cos(m3)* sin(7 * m2), X,
			cos(m3)* sin(m3)* cos(7 * m2) - cos(m3) * sin(m3), sin(m3)* sin(m3)* cos(7 * m2) + cos(m3) * cos(m3), sin(m3)* sin(7 * m2), Y,
			-cos(m3) * sin(7 * m2), -sin(m3) * sin(7 * m2), cos(7 * m2), Z,
			0, 0, 0, 1;
		T03 = T01 * T12*T23;
		Tr = T03.inverse() *T07*T67N;

		m5 = acos(-Tr(1, 2));
		m4 = asin(-Tr(0, 2) / sin(m5));
		m6 = asin(-Tr(1, 1) / sin(m5));

		r = Ldd / (tan(m2 / 2));
		r1 = r - R * cos(m3);
		r2 = r - R * sin(m3);
		r3 = r + R * cos(m3);
		r4 = r + R * sin(m3);

		l1 = r1 * 2 * sin(m2 / 2) * 6;
		l2 = r2 * 2 * sin(m2 / 2) * 6;
		l3 = r3 * 2 * sin(m2 / 2) * 6;
		l4 = r4 * 2 * sin(m2 / 2) * 6;

		m_dj_1 = (l1 - l10) / 4.5;
		m_dj_2 = (l2 - l20) / 4.5;
		m_dj_3 = (l3 - l30) / 4.5;
		m_dj_4 = (l4 - l40) / 4.5;

		T_theta_addh[0] = m1; T_theta_addh[1] = m2; T_theta_addh[2] = m3;
		T_theta_addh[3] = m4; T_theta_addh[4] = m5; T_theta_addh[5] = m6;
		T_theta_addh[6] = m_dj_1; T_theta_addh[7] = m_dj_2; T_theta_addh[8] = m_dj_3;
		T_theta_addh[9] = m_dj_4; T_theta_addh[10] = h;

		return T_theta_addh;
	}
	MatrixXd Jacobian_EndToBase()//输入六个角度theta_1-theta_6，输出雅可比逆解Tjac_Env
	{
		MatrixXd Tjac_Env_EndToBase(8, 6), Tjac(6, 6), Tjac_EndToBase(6, 6), T_jtodj(8, 6);

		Tjac_EndToBase(0, 0) = cos(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2)
			+ 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - sin(theta_1)*(40 * cos(theta_2 / 2) *cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2)
				+ 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2))
			- (93 * cos(7 * theta_2)*sin(theta_1)) / 2 - (987 * cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3) *sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4 - (987 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1)
				- sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 - 436 * sin(theta_1) + (93 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2;
		Tjac_EndToBase(0, 1) = (987 * cos(theta_5)*(7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3) + 7 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)*sin(theta_3))) / 4 - (651 * sin(7 * theta_2)*cos(theta_1)) / 2 - cos(theta_1)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*sin((3 * theta_2) / 2)
			+ 20 * cos((3 * theta_2) / 2)*sin(theta_2 / 2) + 100 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 20 * cos((5 * theta_2) / 2)*sin(theta_2 / 2) + 140 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 20 * cos((7 * theta_2) / 2)*sin(theta_2 / 2) + 180 * cos(theta_2 / 2)*sin((9 * theta_2) / 2)
			+ 20 * cos((9 * theta_2) / 2)*sin(theta_2 / 2) + 220 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 20 * cos((11 * theta_2) / 2)*sin(theta_2 / 2) + 260 * cos(theta_2 / 2)*sin((13 * theta_2) / 2) + 20 * cos((13 * theta_2) / 2)*sin(theta_2 / 2)) - (987 * sin(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*cos(theta_1)
				- 7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_1)*cos(theta_3) + 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3)))) / 4 + sin(theta_1)*sin(theta_3)*(20 * cos(theta_2 / 2)*cos(theta_2 / 2) - 20 * sin(theta_2 / 2) *sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*cos((3 * theta_2) / 2)
					+ 100 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 140 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 180 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 220 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 260 * cos(theta_2 / 2)*cos((13 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((3 * theta_2) / 2)
					- 20 * sin(theta_2 / 2)*sin((5 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((7 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((9 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((11 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((13 * theta_2) / 2)) + (651 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2;
		Tjac_EndToBase(0, 2) = (987 * sin(theta_5)*(sin(theta_4)*(sin(theta_1)*(cos(theta_3) *cos(theta_3) - sin(theta_3) * sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_3)*cos(theta_4)*sin(theta_1))) / 4 + (987 * cos(theta_5)*(sin(theta_1)*(2 * cos(theta_3)*sin(theta_3)
			- 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))) / 4 + cos(theta_3)*sin(theta_1)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2)
				+ 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)) / 2;
		Tjac_EndToBase(0, 3) = -(987 * sin(theta_5)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4;
		Tjac_EndToBase(0, 4) = (987 * sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4 + (987 * cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4;
		Tjac_EndToBase(0, 5) = 0;
		Tjac_EndToBase(1, 0) = 436 * cos(theta_1) + (93 * cos(7 * theta_2)*cos(theta_1)) / 2 - (987 * cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4 + (987 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1)
			+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4 + cos(theta_1)*(40 * cos(theta_2 / 2) * cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2)
				+ 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + sin(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2)
					+ 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2;
		Tjac_EndToBase(1, 1) = (987 * cos(theta_5)*(7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3) - 7 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)*sin(theta_3))) / 4 - (651 * sin(7 * theta_2)*sin(theta_1)) / 2 - sin(theta_1)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 20 * cos((3 * theta_2) / 2)*sin(theta_2 / 2)
			+ 100 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 20 * cos((5 * theta_2) / 2)*sin(theta_2 / 2) + 140 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 20 * cos((7 * theta_2) / 2)*sin(theta_2 / 2) + 180 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 20 * cos((9 * theta_2) / 2)*sin(theta_2 / 2) + 220 * cos(theta_2 / 2)*sin((11 * theta_2) / 2)
			+ 20 * cos((11 * theta_2) / 2)*sin(theta_2 / 2) + 260 * cos(theta_2 / 2)*sin((13 * theta_2) / 2) + 20 * cos((13 * theta_2) / 2)*sin(theta_2 / 2)) - (987 * sin(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*sin(theta_1) + 7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_1) - 7 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3)))) / 4
			- cos(theta_1)*sin(theta_3)*(20 * cos(theta_2 / 2)*cos(theta_2 / 2) - 20 * sin(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 100 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 140 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 180 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 220 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 260 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)
				- 20 * sin(theta_2 / 2)*sin((3 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((5 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((7 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((9 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((11 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((13 * theta_2) / 2)) - (651 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2;
		Tjac_EndToBase(1, 2) = -(987 * sin(theta_5)*(sin(theta_4)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*cos(theta_4))) / 4 - (987 * cos(theta_5)*(cos(theta_1)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))) / 4
			- cos(theta_1)*cos(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - (93 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)) / 2;
		Tjac_EndToBase(1, 3) = -(987 * sin(theta_5)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4;
		Tjac_EndToBase(1, 4) = (987 * cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 - (987 * sin(theta_5)*(cos(theta_1)*(cos(theta_3)* cos(theta_3) + cos(7 * theta_2)*sin(theta_3) *sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4;
		Tjac_EndToBase(1, 5) = 0;
		Tjac_EndToBase(2, 0) = 0;
		Tjac_EndToBase(2, 1) = cos(theta_3)*(20 * cos(theta_2 / 2)*cos(theta_2 / 2) - 20 * sin(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 100 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 140 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 180 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 220 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 260 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)
			- 20 * sin(theta_2 / 2)*sin((3 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((5 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((7 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((9 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((11 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((13 * theta_2) / 2)) + (651 * cos(7 * theta_2)*cos(theta_3)) / 2 + (987 * sin(theta_5)*(7 * cos(7 * theta_2)*cos(theta_3)*cos(theta_4)
				+ 7 * sin(7 * theta_2)*cos(theta_3) * cos(theta_3) * sin(theta_4))) / 4 + (6909 * sin(7 * theta_2)*cos(theta_3)*cos(theta_5)*sin(theta_3)) / 4;
		Tjac_EndToBase(2, 2) = (987 * cos(theta_5)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)* cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))) / 4 - (93 * sin(7 * theta_2)*sin(theta_3)) / 2 - (987 * sin(theta_5)*(sin(theta_4)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_4)*sin(theta_3))) / 4 - sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2));
		Tjac_EndToBase(2, 3) = -(987 * sin(theta_5)*(cos(theta_4)*(sin(theta_3)* sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 4;
		Tjac_EndToBase(2, 4) = -(987 * cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 4 - (987 * sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 4;
		Tjac_EndToBase(2, 5) = 0;
		Tjac_EndToBase(3, 0) = 0;
		Tjac_EndToBase(3, 1) = 7 * cos(theta_3)*sin(theta_1);
		Tjac_EndToBase(3, 2) = cos(theta_1) - cos(7 * theta_2)*cos(theta_1) - sin(7 * theta_2)*sin(theta_1)*sin(theta_3);
		Tjac_EndToBase(3, 3) = sin(7 * theta_2)*cos(theta_1)*sin(theta_3) - sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3));
		Tjac_EndToBase(3, 4) = cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)) - sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3));
		Tjac_EndToBase(3, 5) = sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))) - cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3));
		Tjac_EndToBase(4, 0) = 0;
		Tjac_EndToBase(4, 1) = -7 * cos(theta_1)*cos(theta_3);
		Tjac_EndToBase(4, 2) = sin(theta_1) - cos(7 * theta_2)*sin(theta_1) + sin(7 * theta_2)*cos(theta_1)*sin(theta_3);
		Tjac_EndToBase(4, 3) = cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3);
		Tjac_EndToBase(4, 4) = -sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1));
		Tjac_EndToBase(4, 5) = cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)));
		Tjac_EndToBase(5, 0) = 1;
		Tjac_EndToBase(5, 1) = -7 * sin(theta_3);
		Tjac_EndToBase(5, 2) = -sin(7 * theta_2)*cos(theta_3);
		Tjac_EndToBase(5, 3) = cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3);
		Tjac_EndToBase(5, 4) = -cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_4);
		Tjac_EndToBase(5, 5) = cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4));

		//cout << Tjac_EndToBase << endl;
		T_jtodj << 1, 0, 0, 0, 0, 0,
			0, 0, 0, 0, 0, 0,
			0, 0, 0, 0, 0, 0,
			0, 0, 0, 0, 0, 0,
			0, 0, 0, 0, 0, 0,
			0, 0, 0, 1, 0, 0,
			0, 0, 0, 0, 1, 0,
			0, 0, 0, 0, 0, 1;

		T_jtodj(1, 1) = -6 * (Ldd*sin(theta_2 / 2) + R * cos(theta_3)*cos(theta_2 / 2)) / 4.5;
		T_jtodj(1, 2) = 12 * R*sin(theta_3)*sin(theta_2 / 2) / 4.5;
		T_jtodj(2, 1) = -6 * (Ldd*sin(theta_2 / 2) - R * sin(theta_3)*cos(theta_2 / 2)) / 4.5;
		T_jtodj(2, 2) = -12 * R*cos(theta_3)*sin(theta_2 / 2) / 4.5;
		T_jtodj(3, 1) = 6 * (-Ldd * sin(theta_2 / 2) + R * cos(theta_3)*cos(theta_2 / 2)) / 4.5;
		T_jtodj(3, 2) = -12 * R*sin(theta_3)*sin(theta_2 / 2) / 4.5;
		T_jtodj(4, 1) = 6 * (-Ldd * sin(theta_2 / 2) + R * sin(theta_3)*cos(theta_2 / 2)) / 4.5;
		T_jtodj(4, 2) = 12 * R*cos(theta_3)*sin(theta_2 / 2) / 4.5;

		Tjac_Env_EndToBase = T_jtodj * Tjac_EndToBase.inverse();
		return Tjac_Env_EndToBase;
	}
	MatrixXd Jacobian_CamToBase()//输入六个角度theta_1-theta_6，输出雅可比逆解Tjac_Env(6,6)
	{
		MatrixXd Tjac_CamToBase_Env(8, 6), Tjac_CamToBase(6, 6);
		Tjac_CamToBase(0, 0) = -(15 * sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2
			- sin(theta_1)*(40 * cos(theta_2 / 2)*cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) - (93 * cos(7 * theta_2)*sin(theta_1)) / 2 - (871 * cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3)
			+ cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4 - (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))
			- sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 - (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))
			+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - 436 * sin(theta_1) + cos(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2)
			+ 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2;
		Tjac_CamToBase(0, 1) = -(101 * cos(theta_6)*(sin(theta_5)*(7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3) + 7 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3) *sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*cos(theta_1) - 7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_1)*cos(theta_3)
			+ 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3))))) / 2 - cos(theta_1)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 20 * cos((3 * theta_2) / 2)*sin(theta_2 / 2) + 100 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 20 * cos((5 * theta_2) / 2)*sin(theta_2 / 2)
			+ 140 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 20 * cos((7 * theta_2) / 2)*sin(theta_2 / 2) + 180 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 20 * cos((9 * theta_2) / 2)*sin(theta_2 / 2) + 220 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 20 * cos((11 * theta_2) / 2)*sin(theta_2 / 2) + 260 * cos(theta_2 / 2)*sin((13 * theta_2) / 2) + 20 * cos((13 * theta_2) / 2)*sin(theta_2 / 2))
			- (15 * sin(theta_6)*(sin(theta_5)*(7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3) + 7 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3) *sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*cos(theta_1) - 7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_1)*cos(theta_3) + 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3))))) / 2 - (651 * sin(7 * theta_2)*cos(theta_1)) / 2
			+ (871 * cos(theta_5)*(7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3) + 7 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)*sin(theta_3))) / 4 - (15 * cos(theta_6)*(sin(theta_4)*(7 * sin(7 * theta_2)*cos(theta_1) - 7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) + cos(theta_4)*(7 * cos(7 * theta_2)*cos(theta_1)*cos(theta_3) + 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3)))) / 2
			- (871 * sin(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*cos(theta_1) - 7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_1)*cos(theta_3) + 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3)))) / 4 - (101 * sin(theta_6)*(sin(theta_4)*(7 * sin(7 * theta_2)*cos(theta_1) - 7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) + cos(theta_4)*(7 * cos(7 * theta_2)*cos(theta_1)*cos(theta_3)
			+ 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3)))) / 2 + sin(theta_1)*sin(theta_3)*(20 * cos(theta_2 / 2)*cos(theta_2 / 2) - 20 * sin(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 100 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 140 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 180 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 220 * cos(theta_2 / 2)*cos((11 * theta_2) / 2)
			+ 260 * cos(theta_2 / 2)*cos((13 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((3 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((5 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((7 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((9 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((11 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((13 * theta_2) / 2)) + (651 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2;
		Tjac_CamToBase(0, 2) = -(15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_3)*cos(theta_4)*sin(theta_1)) - sin(theta_5)*(sin(theta_1)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2
			- (15 * cos(theta_6)*(cos(theta_4)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_4))) / 2 + (871 * sin(theta_5)*(sin(theta_4)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))
			- sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_3)*cos(theta_4)*sin(theta_1))) / 4 - (101 * sin(theta_6)*(cos(theta_4)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_4))) / 2 + (871 * cos(theta_5)*(sin(theta_1)*(2 * cos(theta_3)*sin(theta_3)
			- 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))) / 4 - (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_3)*cos(theta_4)*sin(theta_1)) - sin(theta_5)*(sin(theta_1)*(2 * cos(theta_3)*sin(theta_3)
			- 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + cos(theta_3)*sin(theta_1)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2)
			+ 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)) / 2;
		Tjac_CamToBase(0, 3) = -(15 * cos(theta_6)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3)
			- cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4 + (101 * sin(theta_6)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (101 * cos(theta_5)*cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1)
			+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (15 * cos(theta_5)*sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2;
		Tjac_CamToBase(0, 4) = -(871 * sin(theta_5)*(sin(theta_1)*(cos(theta_3)* cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4 + (871 * cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4
			- (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3)
			+ cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2;
		Tjac_CamToBase(0, 5) = -(101 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (15 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3)
			- cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (15 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))
			+ sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + (101 * sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)* cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2;
		Tjac_CamToBase(1, 0) = 436 * cos(theta_1) + (93 * cos(7 * theta_2)*cos(theta_1)) / 2 - (871 * cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))
			+ sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3)
			- cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2
			+ (15 * sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + cos(theta_1)*(40 * cos(theta_2 / 2)*cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2)
			+ 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + sin(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2)
			+ 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2;


		Tjac_CamToBase(1, 1) = (101 * cos(theta_6)*(sin(theta_5)*(7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3) - 7 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*sin(theta_1) + 7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_1) - 7 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3))))) / 2 - (15 * sin(theta_6)*(sin(theta_5)*(7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)
			- 7 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*sin(theta_1) + 7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_1) - 7 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3))))) / 2 - sin(theta_1)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 20 * cos((3 * theta_2) / 2)*sin(theta_2 / 2) + 100 * cos(theta_2 / 2)*sin((5 * theta_2) / 2)
			+ 20 * cos((5 * theta_2) / 2)*sin(theta_2 / 2) + 140 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 20 * cos((7 * theta_2) / 2)*sin(theta_2 / 2) + 180 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 20 * cos((9 * theta_2) / 2)*sin(theta_2 / 2) + 220 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 20 * cos((11 * theta_2) / 2)*sin(theta_2 / 2) + 260 * cos(theta_2 / 2)*sin((13 * theta_2) / 2) + 20 * cos((13 * theta_2) / 2)*sin(theta_2 / 2)) - (651 * sin(7 * theta_2)*sin(theta_1)) / 2
			+ (871 * cos(theta_5)*(7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3) - 7 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)*sin(theta_3))) / 4 - (15 * cos(theta_6)*(sin(theta_4)*(7 * sin(7 * theta_2)*sin(theta_1) + 7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_1) - 7 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3)))) / 2 - (871 * sin(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*sin(theta_1) + 7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_1)
			- 7 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3)))) / 4 - (101 * sin(theta_6)*(sin(theta_4)*(7 * sin(7 * theta_2)*sin(theta_1) + 7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_1) - 7 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3)))) / 2 - cos(theta_1)*sin(theta_3)*(20 * cos(theta_2 / 2)*cos(theta_2 / 2) - 20 * sin(theta_2 / 2)* sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 100 * cos(theta_2 / 2)*cos((5 * theta_2) / 2)
			+ 140 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 180 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 220 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 260 * cos(theta_2 / 2)*cos((13 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((3 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((5 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((7 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((9 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((11 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((13 * theta_2) / 2)) - (651 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2;


		Tjac_CamToBase(1, 2) = (15 * cos(theta_6)*(cos(theta_4)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_4))) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*cos(theta_4))) / 4
			+ (101 * sin(theta_6)*(cos(theta_4)*(cos(theta_1)*(cos(theta_3)* cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)* cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_4))) / 2 - (871 * cos(theta_5)*(cos(theta_1)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))) / 4 + (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)* sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))
			+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*cos(theta_4)) - sin(theta_5)*(cos(theta_1)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*cos(theta_4)) - sin(theta_5)*(cos(theta_1)*(2 * cos(theta_3)*sin(theta_3)
			- 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - cos(theta_1)*cos(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - (93 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)) / 2;


		Tjac_CamToBase(1, 3) = (15 * cos(theta_6)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 + (101 * sin(theta_6)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1)
			- sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (101 * cos(theta_5)*cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (15 * cos(theta_5)*sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3)
			- cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2;


		Tjac_CamToBase(1, 4) = (871 * cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 - (871 * sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4 + (101 * cos(theta_6)*(cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1)
			- sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - (15 * sin(theta_6)*(cos(theta_5)*(cos(theta_1)*(cos(theta_3) * cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2;


		Tjac_CamToBase(1, 5) = (101 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (15 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (15 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))
			+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - (101 * sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2;


		Tjac_CamToBase(2, 0) = 0;
		Tjac_CamToBase(2, 1) = cos(theta_3)*(20 * cos(theta_2 / 2)*cos(theta_2 / 2) - 20 * sin(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 100 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 140 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 180 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 220 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 260 * cos(theta_2 / 2)*cos((13 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((3 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((5 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((7 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((9 * theta_2) / 2)
			- 20 * sin(theta_2 / 2)*sin((11 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((13 * theta_2) / 2)) - (101 * cos(theta_6)*(cos(theta_5)*(7 * cos(7 * theta_2)*cos(theta_3)*cos(theta_4) + 7 * sin(7 * theta_2)*cos(theta_3)*cos(theta_3) * sin(theta_4)) - 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_3)*sin(theta_5))) / 2 + (15 * sin(theta_6)*(cos(theta_5)*(7 * cos(7 * theta_2)*cos(theta_3)*cos(theta_4) + 7 * sin(7 * theta_2)*cos(theta_3)*cos(theta_3) * sin(theta_4)) - 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_3)*sin(theta_5))) / 2 + (651 * cos(7 * theta_2)*cos(theta_3)) / 2 + (15 * cos(theta_6)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_4)
			- 7 * sin(7 * theta_2)*cos(theta_3) *cos(theta_3) * cos(theta_4))) / 2 + (871 * sin(theta_5)*(7 * cos(7 * theta_2)*cos(theta_3)*cos(theta_4) + 7 * sin(7 * theta_2)*cos(theta_3) *cos(theta_3) * sin(theta_4))) / 4 + (101 * sin(theta_6)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_4) - 7 * sin(7 * theta_2)*cos(theta_3)*cos(theta_3) * cos(theta_4))) / 2 + (6097 * sin(7 * theta_2)*cos(theta_3)*cos(theta_5)*sin(theta_3)) / 4;


		Tjac_CamToBase(2, 2) = (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_4)*sin(theta_3)) + sin(theta_5)*(cos(theta_3)*cos(theta_3) - sin(theta_3)* sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)))) / 2 - sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2)
			+ 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_4)*sin(theta_3)) + sin(theta_5)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)))) / 2 - (93 * sin(7 * theta_2)*sin(theta_3)) / 2 + (15 * cos(theta_6)*(cos(theta_4)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*sin(theta_3)*sin(theta_4))) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(2 * cos(theta_3)*sin(theta_3)
			- 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_4)*sin(theta_3))) / 4 + (101 * sin(theta_6)*(cos(theta_4)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*sin(theta_3)*sin(theta_4))) / 2 + (871 * cos(theta_5)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))) / 4;


		Tjac_CamToBase(2, 3) = (101 * cos(theta_5)*cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 - (871 * sin(theta_5)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 4 - (101 * sin(theta_6)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 2 - (15 * cos(theta_6)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 2
			- (15 * cos(theta_5)*sin(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2;


		Tjac_CamToBase(2, 4) = (15 * sin(theta_6)*(sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) - cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) - cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (871 * sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 4 - (871 * cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 4;


		Tjac_CamToBase(2, 5) = (101 * cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 - (15 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)* sin(theta_3) + cos(7 * theta_2)*cos(theta_3) *cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 - (15 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (101 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)* sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2;


		Tjac_CamToBase(3, 0) = 0;
		Tjac_CamToBase(3, 1) = 7 * cos(theta_3)*sin(theta_1);
		Tjac_CamToBase(3, 2) = cos(theta_1) - cos(7 * theta_2)*cos(theta_1) - sin(7 * theta_2)*sin(theta_1)*sin(theta_3);
		Tjac_CamToBase(3, 3) = sin(7 * theta_2)*cos(theta_1)*sin(theta_3) - sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3));
		Tjac_CamToBase(3, 4) = cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)) - sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3));
		Tjac_CamToBase(3, 5) = sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))) - cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3));
		Tjac_CamToBase(4, 0) = 0;
		Tjac_CamToBase(4, 1) = -7 * cos(theta_1)*cos(theta_3);
		Tjac_CamToBase(4, 2) = sin(theta_1) - cos(7 * theta_2)*sin(theta_1) + sin(7 * theta_2)*cos(theta_1)*sin(theta_3);
		Tjac_CamToBase(4, 3) = cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3);
		Tjac_CamToBase(4, 4) = -sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1));
		Tjac_CamToBase(4, 5) = cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)));
		Tjac_CamToBase(5, 0) = 1;
		Tjac_CamToBase(5, 1) = -7 * sin(theta_3);
		Tjac_CamToBase(5, 2) = -sin(7 * theta_2)*cos(theta_3);
		Tjac_CamToBase(5, 3) = cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3);
		Tjac_CamToBase(5, 4) = -cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_4);
		Tjac_CamToBase(5, 5) = cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4));
		
		// cout << Tjac_CamToBase << endl;
		
		Tjac_CamToBase_Env = Tjac_CamToBase.inverse();
		return Tjac_CamToBase_Env;
	}

	MatrixXd Jacobian_CamToBase_addh()//输入六个角度theta_1-theta_6，输出雅可比逆解Tjac_Env(6,6)
	{
		MatrixXd Tjac_CamToBase_Env(8, 6), Tjac_CamToBase(6, 6);
		Tjac_CamToBase(0, 0) = -(15 * sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2
			- sin(theta_1)*(40 * cos(theta_2 / 2)*cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) - (93 * cos(7 * theta_2)*sin(theta_1)) / 2 - (871 * cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3)
			+ cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4 - (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))
			- sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 - (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))
			+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - 436 * sin(theta_1) + cos(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2)
			+ 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2;
		Tjac_CamToBase(0, 1) = -(101 * cos(theta_6)*(sin(theta_5)*(7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3) + 7 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3) *sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*cos(theta_1) - 7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_1)*cos(theta_3)
			+ 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3))))) / 2 - cos(theta_1)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 20 * cos((3 * theta_2) / 2)*sin(theta_2 / 2) + 100 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 20 * cos((5 * theta_2) / 2)*sin(theta_2 / 2)
			+ 140 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 20 * cos((7 * theta_2) / 2)*sin(theta_2 / 2) + 180 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 20 * cos((9 * theta_2) / 2)*sin(theta_2 / 2) + 220 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 20 * cos((11 * theta_2) / 2)*sin(theta_2 / 2) + 260 * cos(theta_2 / 2)*sin((13 * theta_2) / 2) + 20 * cos((13 * theta_2) / 2)*sin(theta_2 / 2))
			- (15 * sin(theta_6)*(sin(theta_5)*(7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3) + 7 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3) *sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*cos(theta_1) - 7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_1)*cos(theta_3) + 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3))))) / 2 - (651 * sin(7 * theta_2)*cos(theta_1)) / 2
			+ (871 * cos(theta_5)*(7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3) + 7 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)*sin(theta_3))) / 4 - (15 * cos(theta_6)*(sin(theta_4)*(7 * sin(7 * theta_2)*cos(theta_1) - 7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) + cos(theta_4)*(7 * cos(7 * theta_2)*cos(theta_1)*cos(theta_3) + 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3)))) / 2
			- (871 * sin(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*cos(theta_1) - 7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_1)*cos(theta_3) + 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3)))) / 4 - (101 * sin(theta_6)*(sin(theta_4)*(7 * sin(7 * theta_2)*cos(theta_1) - 7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) + cos(theta_4)*(7 * cos(7 * theta_2)*cos(theta_1)*cos(theta_3)
			+ 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3)))) / 2 + sin(theta_1)*sin(theta_3)*(20 * cos(theta_2 / 2)*cos(theta_2 / 2) - 20 * sin(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 100 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 140 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 180 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 220 * cos(theta_2 / 2)*cos((11 * theta_2) / 2)
			+ 260 * cos(theta_2 / 2)*cos((13 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((3 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((5 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((7 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((9 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((11 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((13 * theta_2) / 2)) + (651 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2;
		Tjac_CamToBase(0, 2) = -(15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_3)*cos(theta_4)*sin(theta_1)) - sin(theta_5)*(sin(theta_1)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2
			- (15 * cos(theta_6)*(cos(theta_4)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_4))) / 2 + (871 * sin(theta_5)*(sin(theta_4)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))
			- sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_3)*cos(theta_4)*sin(theta_1))) / 4 - (101 * sin(theta_6)*(cos(theta_4)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_4))) / 2 + (871 * cos(theta_5)*(sin(theta_1)*(2 * cos(theta_3)*sin(theta_3)
			- 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))) / 4 - (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_3)*cos(theta_4)*sin(theta_1)) - sin(theta_5)*(sin(theta_1)*(2 * cos(theta_3)*sin(theta_3)
			- 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + cos(theta_3)*sin(theta_1)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2)
			+ 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*cos(theta_3)*sin(theta_1)) / 2;
		Tjac_CamToBase(0, 3) = -(15 * cos(theta_6)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3)
			- cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4 + (101 * sin(theta_6)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (101 * cos(theta_5)*cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1)
			+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (15 * cos(theta_5)*sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2;
		Tjac_CamToBase(0, 4) = -(871 * sin(theta_5)*(sin(theta_1)*(cos(theta_3)* cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4 + (871 * cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4
			- (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3)
			+ cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2;
		Tjac_CamToBase(0, 5) = -(101 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (15 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3)
			- cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (15 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))
			+ sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + (101 * sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)* cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2;
		Tjac_CamToBase(1, 0) =436 * cos(theta_1) - (52 * (tan(theta_1 - pi / 2)*tan(theta_1 - pi / 2) + 1)) / (tan(theta_1 - pi / 2)*tan(theta_1 - pi / 2)) + (93 * cos(7 * theta_2)*cos(theta_1)) / 2 - (891 * cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*
			(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (891 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))
			 - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3) + cos(7 * theta_2)*sin(theta_3) *sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - 
			 cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + (15 * sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 
			 + cos(theta_1)*(40 * cos(theta_2 / 2)* cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + sin(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) +
			  40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2;

		Tjac_CamToBase(1, 1) = (101 * cos(theta_6)*(sin(theta_5)*(7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3) - 7 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*sin(theta_1) + 7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_1) - 7 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3))))) / 2 - (15 * sin(theta_6)*(sin(theta_5)*(7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3)
			- 7 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*sin(theta_1) + 7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_1) - 7 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3))))) / 2 - sin(theta_1)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 20 * cos((3 * theta_2) / 2)*sin(theta_2 / 2) + 100 * cos(theta_2 / 2)*sin((5 * theta_2) / 2)
			+ 20 * cos((5 * theta_2) / 2)*sin(theta_2 / 2) + 140 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 20 * cos((7 * theta_2) / 2)*sin(theta_2 / 2) + 180 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 20 * cos((9 * theta_2) / 2)*sin(theta_2 / 2) + 220 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 20 * cos((11 * theta_2) / 2)*sin(theta_2 / 2) + 260 * cos(theta_2 / 2)*sin((13 * theta_2) / 2) + 20 * cos((13 * theta_2) / 2)*sin(theta_2 / 2)) - (651 * sin(7 * theta_2)*sin(theta_1)) / 2
			+ (871 * cos(theta_5)*(7 * cos(7 * theta_2)*sin(theta_1)*sin(theta_3) - 7 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)*sin(theta_3))) / 4 - (15 * cos(theta_6)*(sin(theta_4)*(7 * sin(7 * theta_2)*sin(theta_1) + 7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_1) - 7 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3)))) / 2 - (871 * sin(theta_5)*(cos(theta_4)*(7 * sin(7 * theta_2)*sin(theta_1) + 7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_1)
			- 7 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3)))) / 4 - (101 * sin(theta_6)*(sin(theta_4)*(7 * sin(7 * theta_2)*sin(theta_1) + 7 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_1) - 7 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3)))) / 2 - cos(theta_1)*sin(theta_3)*(20 * cos(theta_2 / 2)*cos(theta_2 / 2) - 20 * sin(theta_2 / 2)* sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 100 * cos(theta_2 / 2)*cos((5 * theta_2) / 2)
			+ 140 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 180 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 220 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 260 * cos(theta_2 / 2)*cos((13 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((3 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((5 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((7 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((9 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((11 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((13 * theta_2) / 2)) - (651 * cos(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2;


		Tjac_CamToBase(1, 2) = (15 * cos(theta_6)*(cos(theta_4)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_4))) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*cos(theta_4))) / 4
			+ (101 * sin(theta_6)*(cos(theta_4)*(cos(theta_1)*(cos(theta_3)* cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)* cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_4))) / 2 - (871 * cos(theta_5)*(cos(theta_1)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))) / 4 + (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)* sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))
			+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*cos(theta_4)) - sin(theta_5)*(cos(theta_1)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)*cos(theta_4)) - sin(theta_5)*(cos(theta_1)*(2 * cos(theta_3)*sin(theta_3)
			- 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - cos(theta_1)*cos(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - (93 * sin(7 * theta_2)*cos(theta_1)*cos(theta_3)) / 2;


		Tjac_CamToBase(1, 3) = (15 * cos(theta_6)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 + (101 * sin(theta_6)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1)
			- sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (101 * cos(theta_5)*cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (15 * cos(theta_5)*sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3)
			- cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2;


		Tjac_CamToBase(1, 4) = (871 * cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 - (871 * sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4 + (101 * cos(theta_6)*(cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1)
			- sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - (15 * sin(theta_6)*(cos(theta_5)*(cos(theta_1)*(cos(theta_3) * cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2;


		Tjac_CamToBase(1, 5) = (101 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (15 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 - (15 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))
			+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - (101 * sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2;


		Tjac_CamToBase(2, 0) = 0;
		Tjac_CamToBase(2, 1) = cos(theta_3)*(20 * cos(theta_2 / 2)*cos(theta_2 / 2) - 20 * sin(theta_2 / 2)*sin(theta_2 / 2) + 60 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 100 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 140 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 180 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 220 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 260 * cos(theta_2 / 2)*cos((13 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((3 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((5 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((7 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((9 * theta_2) / 2)
			- 20 * sin(theta_2 / 2)*sin((11 * theta_2) / 2) - 20 * sin(theta_2 / 2)*sin((13 * theta_2) / 2)) - (101 * cos(theta_6)*(cos(theta_5)*(7 * cos(7 * theta_2)*cos(theta_3)*cos(theta_4) + 7 * sin(7 * theta_2)*cos(theta_3)*cos(theta_3) * sin(theta_4)) - 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_3)*sin(theta_5))) / 2 + (15 * sin(theta_6)*(cos(theta_5)*(7 * cos(7 * theta_2)*cos(theta_3)*cos(theta_4) + 7 * sin(7 * theta_2)*cos(theta_3)*cos(theta_3) * sin(theta_4)) - 7 * sin(7 * theta_2)*cos(theta_3)*sin(theta_3)*sin(theta_5))) / 2 + (651 * cos(7 * theta_2)*cos(theta_3)) / 2 + (15 * cos(theta_6)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_4)
			- 7 * sin(7 * theta_2)*cos(theta_3) *cos(theta_3) * cos(theta_4))) / 2 + (871 * sin(theta_5)*(7 * cos(7 * theta_2)*cos(theta_3)*cos(theta_4) + 7 * sin(7 * theta_2)*cos(theta_3) *cos(theta_3) * sin(theta_4))) / 4 + (101 * sin(theta_6)*(7 * cos(7 * theta_2)*cos(theta_3)*sin(theta_4) - 7 * sin(7 * theta_2)*cos(theta_3)*cos(theta_3) * cos(theta_4))) / 2 + (6097 * sin(7 * theta_2)*cos(theta_3)*cos(theta_5)*sin(theta_3)) / 4;


		Tjac_CamToBase(2, 2) = (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_4)*sin(theta_3)) + sin(theta_5)*(cos(theta_3)*cos(theta_3) - sin(theta_3)* sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)))) / 2 - sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2)
			+ 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_4)*sin(theta_3)) + sin(theta_5)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)))) / 2 - (93 * sin(7 * theta_2)*sin(theta_3)) / 2 + (15 * cos(theta_6)*(cos(theta_4)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*sin(theta_3)*sin(theta_4))) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(2 * cos(theta_3)*sin(theta_3)
			- 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_4)*sin(theta_3))) / 4 + (101 * sin(theta_6)*(cos(theta_4)*(2 * cos(theta_3)*sin(theta_3) - 2 * cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*sin(theta_3)*sin(theta_4))) / 2 + (871 * cos(theta_5)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))) / 4;


		Tjac_CamToBase(2, 3) = (101 * cos(theta_5)*cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 - (871 * sin(theta_5)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 4 - (101 * sin(theta_6)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 2 - (15 * cos(theta_6)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 2
			- (15 * cos(theta_5)*sin(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2;


		Tjac_CamToBase(2, 4) = (15 * sin(theta_6)*(sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) - cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) - cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (871 * sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 4 - (871 * cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 4;


		Tjac_CamToBase(2, 5) = (101 * cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 - (15 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)* sin(theta_3) + cos(7 * theta_2)*cos(theta_3) *cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 - (15 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (101 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)* sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2;


		Tjac_CamToBase(3, 0) = 0;
		Tjac_CamToBase(3, 1) = 7 * cos(theta_3)*sin(theta_1);
		Tjac_CamToBase(3, 2) = cos(theta_1) - cos(7 * theta_2)*cos(theta_1) - sin(7 * theta_2)*sin(theta_1)*sin(theta_3);
		Tjac_CamToBase(3, 3) = sin(7 * theta_2)*cos(theta_1)*sin(theta_3) - sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3));
		Tjac_CamToBase(3, 4) = cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)) - sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3));
		Tjac_CamToBase(3, 5) = sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))) - cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3));
		Tjac_CamToBase(4, 0) = 0;
		Tjac_CamToBase(4, 1) = -7 * cos(theta_1)*cos(theta_3);
		Tjac_CamToBase(4, 2) = sin(theta_1) - cos(7 * theta_2)*sin(theta_1) + sin(7 * theta_2)*cos(theta_1)*sin(theta_3);
		Tjac_CamToBase(4, 3) = cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3);
		Tjac_CamToBase(4, 4) = -sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1));
		Tjac_CamToBase(4, 5) = cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)));
		Tjac_CamToBase(5, 0) = 1;
		Tjac_CamToBase(5, 1) = -7 * sin(theta_3);
		Tjac_CamToBase(5, 2) = -sin(7 * theta_2)*cos(theta_3);
		Tjac_CamToBase(5, 3) = cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3);
		Tjac_CamToBase(5, 4) = -cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_4);
		Tjac_CamToBase(5, 5) = cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4));
		
		// cout << Tjac_CamToBase << endl;
		
		Tjac_CamToBase_Env = Tjac_CamToBase.inverse();
		return Tjac_CamToBase_Env;
	}

	MatrixXd vel_maping_joints2motors()
{
    MatrixXd T_jtodj(8, 6);
	T_jtodj << 1, 0, 0, 0, 0, 0,
			0, 0, 0, 0, 0, 0,
			0, 0, 0, 0, 0, 0,
			0, 0, 0, 0, 0, 0,
			0, 0, 0, 0, 0, 0,
			0, 0, 0, 1, 0, 0,
			0, 0, 0, 0, 1, 0,
			0, 0, 0, 0, 0, 1;

	T_jtodj(1, 1) = -6 * (Ldd*sin(theta_2 / 2) + R * cos(theta_3)*cos(theta_2 / 2)) / 4.5;
	T_jtodj(1, 2) = 12 * R*sin(theta_3)*sin(theta_2 / 2) / 4.5;
	T_jtodj(2, 1) = -6 * (Ldd*sin(theta_2 / 2) - R * sin(theta_3)*cos(theta_2 / 2)) / 4.5;
	T_jtodj(2, 2) = -12 * R*cos(theta_3)*sin(theta_2 / 2) / 4.5;
	T_jtodj(3, 1) = 6 * (-Ldd * sin(theta_2 / 2) + R * cos(theta_3)*cos(theta_2 / 2)) / 4.5;
	T_jtodj(3, 2) = -12 * R*sin(theta_3)*sin(theta_2 / 2) / 4.5;
	T_jtodj(4, 1) = 6 * (-Ldd * sin(theta_2 / 2) + R * sin(theta_3)*cos(theta_2 / 2)) / 4.5;
	T_jtodj(4, 2) = 12 * R*cos(theta_3)*sin(theta_2 / 2) / 4.5;

	return T_jtodj;
}
    
	MatrixXd Get_Vel_Translation()
	{
		MatrixXd Tvel_CamToBase(6, 6);
		Tvel_CamToBase(0, 0) = -cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))) + sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))));
		// cout << Tvel_CamToBase(0, 0) << endl;
		Tvel_CamToBase(0, 1) = (7071 * cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 10000 - (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 10000 + (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))
		- cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 10000 - (7071 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 10000;


		Tvel_CamToBase(0, 2) = (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 10000 - (7071 * cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 10000 + (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 10000
		- (7071 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 10000;

        Tvel_CamToBase(0, 3) = 0;
//		Tvel_CamToBase(0, 3) = (cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))) - sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))))*(cos(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2)
//		+ 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (15 * cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (93 * sin(7 * theta_2)*cos(theta_3)) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 4 + (101 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3))
//		+ sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (871 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 4 + (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2) - (cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))
//		- sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))))*(436 * sin(theta_1) + sin(theta_1)*(40 * cos(theta_2 / 2)* cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + (93 * cos(7 * theta_2)*sin(theta_1)) / 2 + (871 * cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4
//		+ (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (101 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3)
//		+ cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - (15 * sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - cos(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2)
//		+ 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - (93 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2);

        Tvel_CamToBase(0, 4) = 0;
//		Tvel_CamToBase(0, 4) = -((7071 * sin(theta_5)*(sin(theta_4)*(sin(theta_3) *sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 10000 + (7071 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 10000 - (7071 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 10000 + (7071 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 10000)*(436 * sin(theta_1) + sin(theta_1)*(40 * cos(theta_2 / 2)* cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2)
//		+ 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + (93 * cos(7 * theta_2)*sin(theta_1)) / 2 + (871 * cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))
//		- sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (101 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2
//		- (15 * sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - cos(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - (93 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2)
//		- ((7071 * cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 10000 + (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 - (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 - (7071 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1)
//		- sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 10000)*(cos(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (15 * cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (93 * sin(7 * theta_2)*cos(theta_3)) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(sin(theta_3)* sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 4
//		+ (101 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)* sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (871 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 4 + (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)* sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2);

        Tvel_CamToBase(0, 5) = 0;
//		Tvel_CamToBase(0, 5) = ((7071 * cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 10000 + (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 + (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 + (7071 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))
//		+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 10000)*(cos(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (15 * cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (93 * sin(7 * theta_2)*cos(theta_3)) / 2
//		- (871 * sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 4 + (101 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (871 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 4 + (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2)
//		- ((7071 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 10000 - (7071 * sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 10000 + (7071 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 10000 + (7071 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 10000)*(436 * sin(theta_1) + sin(theta_1)*(40 * cos(theta_2 / 2)* cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2)
//		+ 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + (93 * cos(7 * theta_2)*sin(theta_1)) / 2 + (871 * cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4
//		+ (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (101 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - (15 * sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))
//		- sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - cos(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - (93 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2);
		
		Tvel_CamToBase(1, 0) = -cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))) - sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))));


			Tvel_CamToBase(1, 1) = (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 - (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 - (7071 * cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3))
				+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 10000 + (7071 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 10000;


			Tvel_CamToBase(1, 2) = (7071 * cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 10000 + (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 + (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))
				- sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 + (7071 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 10000;

            Tvel_CamToBase(1, 3) = 0;
//			Tvel_CamToBase(1, 3) = (cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4)) - sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))))*(436 * cos(theta_1) + (93 * cos(7 * theta_2)*cos(theta_1)) / 2 - (871 * cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4
//				+ (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))
//					- cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + (15 * sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3)
//						+ cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + cos(theta_1)*(40 * cos(theta_2 / 2) *cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2)
//							+ 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + sin(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2) - (cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3)
//								- cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))) + sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))))*(cos(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2)
//									+ 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (15 * cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (93 * sin(7 * theta_2)*cos(theta_3)) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 4
//									+ (101 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (871 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 4 + (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))
//										+ sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2);

            Tvel_CamToBase(1, 4) = 0;
//			Tvel_CamToBase(1, 4) = ((7071 * sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 10000 + (7071 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 10000 - (7071 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 10000 + (7071 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))))
//				/ 10000)*(436 * cos(theta_1) + (93 * cos(7 * theta_2)*cos(theta_1)) / 2 - (871 * cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3)
//					- cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3)
//						- cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + (15 * sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + cos(theta_1)*(40 * cos(theta_2 / 2) *cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2)
//							+ 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + sin(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2)
//				- ((7071 * cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 10000 - (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 10000 + (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))
//					+ sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 10000 - (7071 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 10000)*(cos(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2)
//						+ 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (15 * cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (93 * sin(7 * theta_2)*cos(theta_3)) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 4 + (101 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3))
//							+ sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (871 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 4 + (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2);

            Tvel_CamToBase(1, 5) = 0;
//			Tvel_CamToBase(1, 5) = ((7071 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 10000 - (7071 * sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 10000 + (7071 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 10000 + (7071 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))))
//				/ 10000)*(436 * cos(theta_1) + (93 * cos(7 * theta_2)*cos(theta_1)) / 2 - (871 * cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))
//					+ sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1)
//						+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + (15 * sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))))
//					/ 2 + cos(theta_1)*(40 * cos(theta_2 / 2) *cos(theta_2 / 2) + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + sin(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2)
//						+ 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2) + ((7071 * cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 10000 - (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 10000 - (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1)
//							+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 10000 + (7071 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))))
//							/ 10000)*(cos(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (15 * cos(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (93 * sin(7 * theta_2)*cos(theta_3)) / 2 - (871 * sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3)
//								+ cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 4 + (101 * sin(theta_6)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 2 + (871 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 4 + (101 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2 - (15 * sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3)
//									+ cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 2);




			Tvel_CamToBase(2, 0) = -cos(theta_6)*(cos(theta_4)*(sin(theta_3) *sin(theta_3)  + cos(7 * theta_2)*cos(theta_3) *cos(theta_3) ) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4)) - sin(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3) *sin(theta_3)  + cos(7 * theta_2)*cos(theta_3) *cos(theta_3) ) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)));
			Tvel_CamToBase(2, 1) = (7071 * sin(theta_5)*(sin(theta_4)*(sin(theta_3) *sin(theta_3)  + cos(7 * theta_2)*cos(theta_3) *cos(theta_3) ) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 10000 + (7071 * sin(theta_6)*(cos(theta_4)*(sin(theta_3) *sin(theta_3)  + cos(7 * theta_2)*cos(theta_3) *cos(theta_3) ) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 10000 - (7071 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 10000 + (7071 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3) *sin(theta_3)  + cos(7 * theta_2)*cos(theta_3) *cos(theta_3) ) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 10000;


			Tvel_CamToBase(2, 2) = (7071 * sin(theta_6)*(cos(theta_4)*(sin(theta_3) *sin(theta_3)  + cos(7 * theta_2)*cos(theta_3) *cos(theta_3) ) + sin(7 * theta_2)*cos(theta_3)*sin(theta_4))) / 10000 - (7071 * sin(theta_5)*(sin(theta_4)*(sin(theta_3) *sin(theta_3)  + cos(7 * theta_2)*cos(theta_3) *cos(theta_3) ) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4))) / 10000 + (7071 * cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))) / 10000 + (7071 * cos(theta_6)*(cos(theta_5)*(sin(theta_4)*(sin(theta_3) *sin(theta_3)  + cos(7 * theta_2)*cos(theta_3) *cos(theta_3) ) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4)) + sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)))) / 10000;

            Tvel_CamToBase(2, 3) = 0;
//			Tvel_CamToBase(2, 3) = (cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))) + sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))))*(436 * sin(theta_1) + sin(theta_1)*(40 * cos(theta_2 / 2) *cos(theta_2 / 2)
//				+ 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + (93 * cos(7 * theta_2)*sin(theta_1)) / 2 + (871 * cos(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))
//					- sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (101 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))
//						- cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - (15 * sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - cos(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2)
//							+ 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - (93 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2) - (cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))) - sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) )
//								+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))))*(436 * cos(theta_1) + (93 * cos(7 * theta_2)*cos(theta_1)) / 2 - (871 * cos(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3))
//									+ sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))
//										+ cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + (15 * sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + cos(theta_1)*(40 * cos(theta_2 / 2) *cos(theta_2 / 2)  + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2)
//											+ 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + sin(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2);

          Tvel_CamToBase(2, 4) = 0;
//			Tvel_CamToBase(2, 4) = ((7071 * cos(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 10000 + (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 - (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 - (7071 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)
//				+ cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 10000)*(436 * cos(theta_1) + (93 * cos(7 * theta_2)*cos(theta_1)) / 2 - (871 * cos(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7
//					* theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1)
//						+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + (15 * sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + cos(theta_1)*(40 * cos(theta_2 / 2) *cos(theta_2 / 2)  + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2
//							/ 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + sin(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2) + ((7071 * cos(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)))
//								/ 10000 - (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 10000 + (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 10000 - (7071 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))
//									+ sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 10000)*(436 * sin(theta_1) + sin(theta_1)*(40 * cos(theta_2 / 2) *cos(theta_2 / 2)  + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + (93 * cos(7 * theta_2)*sin(theta_1)) / 2 + (871 * cos(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1)
//										- sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (101 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) )
//											+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - (15 * sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - cos(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2)
//												+ 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - (93 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2);

        Tvel_CamToBase(2, 5) = 0;
//			Tvel_CamToBase(2, 5) = -((7071 * cos(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 10000 + (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 + (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 10000 + (7071 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) )
//				+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 10000)*(436 * cos(theta_1) + (93 * cos(7 * theta_2)*cos(theta_1)) / 2 - (871 * cos(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1)
//					+ sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 2 - (101 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2
//					+ (15 * sin(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 2 + cos(theta_1)*(40 * cos(theta_2 / 2) *cos(theta_2 / 2)  + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + sin(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2)
//						+ 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) + (93 * sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) / 2) - ((7071 * cos(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))) / 10000 - (7071 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))
//							/ 10000 - (7071 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)))) / 10000 + (7071 * cos(theta_6)*(sin(theta_5)*(sin(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))))) / 10000)*(436 * sin(theta_1) + sin(theta_1)*(40 * cos(theta_2 / 2) *cos(theta_2 / 2)  + 40 * cos(theta_2 / 2)*cos((3 * theta_2) / 2)
//								+ 40 * cos(theta_2 / 2)*cos((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*cos((13 * theta_2) / 2)) + (93 * cos(7 * theta_2)*sin(theta_1)) / 2 + (871 * cos(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3))) / 4 + (15 * cos(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (871 * sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3))
//									- sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 4 + (101 * sin(theta_6)*(sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)))) / 2 + (101 * cos(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2
//								- (15 * sin(theta_6)*(sin(theta_5)*(cos(theta_1)*(cos(theta_3) *cos(theta_3)  + cos(7 * theta_2)*sin(theta_3) *sin(theta_3) ) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1))))) / 2 - cos(theta_1)*sin(theta_3)*(40 * cos(theta_2 / 2)*sin(theta_2 / 2) + 40 * cos(theta_2 / 2)*sin((3 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((5 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((7 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((9 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((11 * theta_2) / 2) + 40 * cos(theta_2 / 2)*sin((13 * theta_2) / 2)) - (93 * sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) / 2);




			Tvel_CamToBase(3, 0) = 0;
			Tvel_CamToBase(3, 1) = 0;
			Tvel_CamToBase(3, 2) = 0;
			Tvel_CamToBase(3, 3) = -Tvel_CamToBase(0, 0);
			Tvel_CamToBase(3, 4) = Tvel_CamToBase(0, 1);
			Tvel_CamToBase(3, 5) = Tvel_CamToBase(0, 2);


			Tvel_CamToBase(4, 0) = 0;
			Tvel_CamToBase(4, 1) = 0;
			Tvel_CamToBase(4, 2) = 0;
			Tvel_CamToBase(4, 3) = -Tvel_CamToBase(1, 0);
			Tvel_CamToBase(4, 4) = Tvel_CamToBase(1, 1);
			Tvel_CamToBase(4, 5) = Tvel_CamToBase(1, 2);


			Tvel_CamToBase(5, 0) = 0;
			Tvel_CamToBase(5, 1) = 0;
			Tvel_CamToBase(5, 2) = 0;
			Tvel_CamToBase(5, 3) = -Tvel_CamToBase(2, 0);
			Tvel_CamToBase(5, 4) = Tvel_CamToBase(2, 1);
			Tvel_CamToBase(5, 5) = Tvel_CamToBase(2, 2);

			return Tvel_CamToBase;

	}

	vector<double> kec_motors2End()//输入 theta_1 m_dj_1 m_dj_2 m_dj_3 m_dj_4 theta_4 theta_5 theta_6 输出T07(末端到基座标系位姿矩阵
	{
		double Line_1, Line_2, Line_3, Line_4, Line;

		Line_1 = Line + 4.5 * m_dj_1 / 7;
		Line_2 = Line + 4.5 * m_dj_2 / 7;
		Line_3 = Line + 4.5 * m_dj_3 / 7;
		Line_4 = Line + 4.5 * m_dj_4 / 7;

		if (Line_1 < Line_3)
		{
			theta_3 = atan((Line_2 * Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2 * Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3));
		}
		else if (Line_1 > Line_3)
		{
			theta_3 = atan((Line_2 * Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2 * Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3)) + pi;
		}
		else
		{
			theta_3 = pi;
		}
		theta_2 = 2 * asin((Line_1 + Line_3) / 4 * sqrt((Line_2 * Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3) * (Line_2 * Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3))
			+ (Line_2 * Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3) * (Line_2 * Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3)) / ((Line_4 * Line_1 + Line_4 * Line_3 + Line_2 * Line_1 + Line_2 * Line_3) * R);

		Matrix4d T_01, T_12, T_23, T_34, T_45, T_56, T_67, T_07;
		Matrix3d RotMatrix;
		MatrixXd moveDirection(3, 1), moveDirectionNew(3, 1);
		moveDirection << -1, 0, 0;
		T_01 << cos(theta_1), -sin(theta_1), 0, 0, sin(theta_1), cos(theta_1), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
		T_12 << 0, 0, 1, L1, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
		a = 0, b = 0;
		for (int i = 1; i < 8; i++)
		{
			a = a + 2 * L * cos((theta_2) / 2) * sin(((theta_2) / 2) + (i - 1) * (theta_2));
			b = b + 2 * L * cos((theta_2) / 2) * cos(((theta_2) / 2) + (i - 1) * (theta_2));
		}

		X = a * cos(theta_3); Y = a * sin(theta_3); Z = b;
		T_23 << cos(theta_3) * cos(theta_3) * cos(7 * theta_2) + sin(theta_3) * sin(theta_3), cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), cos(theta_3)* sin(7 * theta_2), X,
			cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), sin(theta_3)* sin(theta_3)* cos(7 * theta_2) + cos(theta_3) * cos(theta_3), sin(theta_3)* sin(7 * theta_2), Y,
			-cos(theta_3) * sin(7 * theta_2), -sin(theta_3) * sin(7 * theta_2), cos(7 * theta_2), Z,
			0, 0, 0, 1;
		T_34 << -sin(theta_4), -cos(theta_4), 0, 0, 0, 0, -1, 0, cos(theta_4), -sin(theta_4), 0, L2, 0, 0, 0, 1;
		T_45 << cos(theta_5), -sin(theta_5), 0, 0, 0, 0, 1, 0, -sin(theta_5), -cos(theta_5), 0, 0, 0, 0, 0, 1;
		T_56 << cos(theta_6), -sin(theta_6), 0, 0, 0, 0, -1, 0, sin(theta_6), cos(theta_6), 0, 0, 0, 0, 0, 1;
		T_67 << 1, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, L3, 0, 0, 0, 1;
		T_07 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56 * T_67;
		RotMatrix << T_07(0, 0), T_07(0, 1), T_07(0, 2), T_07(1, 0), T_07(1, 1), T_07(1, 2), T_07(2, 0), T_07(2, 1), T_07(2, 2);
		moveDirectionNew = RotMatrix * moveDirection;
		vector<double> positionPosture(9, 0);
		positionPosture[0] = T_07(0, 3);
		positionPosture[1] = T_07(1, 3);
		positionPosture[2] = T_07(2, 3);
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > -pi / 2 && atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < pi / 2)
		{
			positionPosture[3] = atan2(T_07(2, 1), T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(T_07(1, 0), T_07(0, 0));
		}
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < -pi / 2 || atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > pi / 2)
		{
			positionPosture[3] = atan2(-T_07(2, 1), -T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(-T_07(1, 0), -T_07(0, 0));
		}
		positionPosture[6] = moveDirectionNew(0);
		positionPosture[7] = moveDirectionNew(1);
		positionPosture[8] = moveDirectionNew(2);
		return positionPosture;
	}

	vector<double> kec_joints2End()//输入 theta_1 theta_2 theta_3 theta_4 theta_5 theta_6 输出T07(末端到基座标系位姿矩阵
	{
		Matrix4d T_01, T_12, T_23, T_34, T_45, T_56, T_67, T_07;
		Matrix3d RotMatrix;
		MatrixXd moveDirection(3, 1), moveDirectionNew(3, 1);
		moveDirection << -1, 0, 0;
		T_01 << cos(theta_1), -sin(theta_1), 0, 0, sin(theta_1), cos(theta_1), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
		T_12 << 0, 0, 1, L1, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
		a = 0, b = 0;
		for (int i = 1; i < 8; i++)
		{
			a = a + 2 * L * cos((theta_2) / 2) * sin(((theta_2) / 2) + (i - 1) * (theta_2));
			b = b + 2 * L * cos((theta_2) / 2) * cos(((theta_2) / 2) + (i - 1) * (theta_2));
		}

		X = a * cos(theta_3); Y = a * sin(theta_3); Z = b;
		T_23 << cos(theta_3) * cos(theta_3) * cos(7 * theta_2) + sin(theta_3) * sin(theta_3), cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), cos(theta_3)* sin(7 * theta_2), X,
			cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), sin(theta_3)* sin(theta_3)* cos(7 * theta_2) + cos(theta_3) * cos(theta_3), sin(theta_3)* sin(7 * theta_2), Y,
			-cos(theta_3) * sin(7 * theta_2), -sin(theta_3) * sin(7 * theta_2), cos(7 * theta_2), Z,
			0, 0, 0, 1;
		T_34 << -sin(theta_4), -cos(theta_4), 0, 0, 0, 0, -1, 0, cos(theta_4), -sin(theta_4), 0, L2, 0, 0, 0, 1;
		T_45 << cos(theta_5), -sin(theta_5), 0, 0, 0, 0, 1, 0, -sin(theta_5), -cos(theta_5), 0, 0, 0, 0, 0, 1;
		T_56 << cos(theta_6), -sin(theta_6), 0, 0, 0, 0, -1, 0, sin(theta_6), cos(theta_6), 0, 0, 0, 0, 0, 1;
		T_67 << 1, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, L3, 0, 0, 0, 1;
		T_07 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56 * T_67;
		RotMatrix << T_07(0, 0), T_07(0, 1), T_07(0, 2), T_07(1, 0), T_07(1, 1), T_07(1, 2), T_07(2, 0), T_07(2, 1), T_07(2, 2);
		moveDirectionNew = RotMatrix * moveDirection;
		vector<double> positionPosture(9, 0);
		positionPosture[0] = T_07(0, 3);
		positionPosture[1] = T_07(1, 3);
		positionPosture[2] = T_07(2, 3);
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > -pi / 2 && atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < pi / 2)
		{
			positionPosture[3] = atan2(T_07(2, 1), T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(T_07(1, 0), T_07(0, 0));
		}
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < -pi / 2 || atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > pi / 2)
		{
			positionPosture[3] = atan2(-T_07(2, 1), -T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(-T_07(1, 0), -T_07(0, 0));
		}
		positionPosture[6] = moveDirectionNew(0);
		positionPosture[7] = moveDirectionNew(1);
		positionPosture[8] = moveDirectionNew(2);
		return positionPosture;
	}

	vector<double> kec_motors2End_addh(int log)//输入 theta_1 m_dj_1 m_dj_2 m_dj_3 m_dj_4 theta_4 theta_5 theta_6 h 输出末端位姿
	{
		double Line_1, Line_2, Line_3, Line_4, Line;

		Line_1 = Line + 4.5 * m_dj_1 / 6;
		Line_2 = Line + 4.5 * m_dj_2 / 6;
		Line_3 = Line + 4.5 * m_dj_3 / 6;
		Line_4 = Line + 4.5 * m_dj_4 / 6;

		if (Line_1 < Line_3)
		{
			theta_3 = atan((Line_2 * Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2 * Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3));
		}
		else if (Line_1 > Line_3)
		{
			theta_3 = atan((Line_2 * Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2 * Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3)) + pi;
		}
		else
		{
			theta_3 = pi;
		}
		theta_2 = 2 * asin((Line_1 + Line_3) / 4 * sqrt((Line_2 * Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3) * (Line_2 * Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3))
			+ (Line_2 * Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3) * (Line_2 * Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3)) / ((Line_4 * Line_1 + Line_4 * Line_3 + Line_2 * Line_1 + Line_2 * Line_3) * R);

		Matrix4d T_H0, T_01, T_12, T_23, T_34, T_45, T_56, T_67, T_07;
		Matrix3d RotMatrix;
		MatrixXd moveDirection(3, 1), moveDirectionNew(3, 1);
		moveDirection << -1, 0, 0;
		if(log == 2 | log == 7){
			double h = 238 - 49.07 / tan(pi / 2 - theta_1);
		}else if(log == 3 | log == 8){
			double h = 238 - 55.41 / tan(pi / 2 - theta_1);
		}else{
			double h = 238 - 50.59 / tan(pi / 2 - theta_1);
		}
		T_H0 << 1,0,0,0, 0,1,0,h, 0,0,1,0, 0,0,0,1;
		T_01 << cos(theta_1), -sin(theta_1), 0, 0, sin(theta_1), cos(theta_1), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
		T_12 << 0, 0, 1, L1, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
		a = 0, b = 0;
		for (int i = 1; i < 8; i++)
		{
			a = a + 2 * L * cos((theta_2) / 2) * sin(((theta_2) / 2) + (i - 1) * (theta_2));
			b = b + 2 * L * cos((theta_2) / 2) * cos(((theta_2) / 2) + (i - 1) * (theta_2));
		}

		X = a * cos(theta_3); Y = a * sin(theta_3); Z = b;
		T_23 << cos(theta_3) * cos(theta_3) * cos(7 * theta_2) + sin(theta_3) * sin(theta_3), cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), cos(theta_3)* sin(7 * theta_2), X,
			cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), sin(theta_3)* sin(theta_3)* cos(7 * theta_2) + cos(theta_3) * cos(theta_3), sin(theta_3)* sin(7 * theta_2), Y,
			-cos(theta_3) * sin(7 * theta_2), -sin(theta_3) * sin(7 * theta_2), cos(7 * theta_2), Z,
			0, 0, 0, 1;
		T_34 << -sin(theta_4), -cos(theta_4), 0, 0, 0, 0, -1, 0, cos(theta_4), -sin(theta_4), 0, L2, 0, 0, 0, 1;
		T_45 << cos(theta_5), -sin(theta_5), 0, 0, 0, 0, 1, 0, -sin(theta_5), -cos(theta_5), 0, 0, 0, 0, 0, 1;
		T_56 << cos(theta_6), -sin(theta_6), 0, 0, 0, 0, -1, 0, sin(theta_6), cos(theta_6), 0, 0, 0, 0, 0, 1;
		T_67 << 1, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, L3, 0, 0, 0, 1;
		T_07 = T_H0 *T_01 * T_12 * T_23 * T_34 * T_45 * T_56 * T_67;
		RotMatrix << T_07(0, 0), T_07(0, 1), T_07(0, 2), T_07(1, 0), T_07(1, 1), T_07(1, 2), T_07(2, 0), T_07(2, 1), T_07(2, 2);
		moveDirectionNew = RotMatrix * moveDirection;
		vector<double> positionPosture(9, 0);
		positionPosture[0] = T_07(0, 3);
		positionPosture[1] = T_07(1, 3);
		positionPosture[2] = T_07(2, 3);
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > -pi / 2 && atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < pi / 2)
		{
			positionPosture[3] = atan2(T_07(2, 1), T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(T_07(1, 0), T_07(0, 0));
		}
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < -pi / 2 || atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > pi / 2)
		{
			positionPosture[3] = atan2(-T_07(2, 1), -T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(-T_07(1, 0), -T_07(0, 0));
		}
		positionPosture[6] = moveDirectionNew(0);
		positionPosture[7] = moveDirectionNew(1);
		positionPosture[8] = moveDirectionNew(2);
		return positionPosture;
	
	}

	vector<double> kec_joints2End_addh(int log)//输入 theta_1 theta_2 theta_3 theta_4 theta_5 theta_6 h 输出末端位姿
	{
		// std::cout<<"-------------------------特殊的运动学----------------------------"<<endl;
		Matrix4d T_H0, T_01, T_12, T_23, T_34, T_45, T_56, T_67, T_07;
		Matrix3d RotMatrix;
		MatrixXd moveDirection(3, 1), moveDirectionNew(3, 1);
		moveDirection << -1, 0, 0;
		if(log == 2 | log == 7){
			double h = 238 - 49.07 / tan(pi / 2 - theta_1);
		}else if(log == 3 | log == 8){
			double h = 238 - 55.41 / tan(pi / 2 - theta_1);
		}else{
			double h = 238 - 50.59 / tan(pi / 2 - theta_1);
		}
		T_H0 << 1,0,0,0, 0,1,0,h, 0,0,1,0, 0,0,0,1;
		T_01 << cos(theta_1), -sin(theta_1), 0, 0, sin(theta_1), cos(theta_1), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
		T_12 << 0, 0, 1, L1, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
		a = 0, b = 0;
		for (int i = 1; i < 8; i++)
		{
			a = a + 2 * L * cos((theta_2) / 2) * sin(((theta_2) / 2) + (i - 1) * (theta_2));
			b = b + 2 * L * cos((theta_2) / 2) * cos(((theta_2) / 2) + (i - 1) * (theta_2));
		}

		X = a * cos(theta_3); Y = a * sin(theta_3); Z = b;
		T_23 << cos(theta_3) * cos(theta_3) * cos(7 * theta_2) + sin(theta_3) * sin(theta_3), cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), cos(theta_3)* sin(7 * theta_2), X,
			cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), sin(theta_3)* sin(theta_3)* cos(7 * theta_2) + cos(theta_3) * cos(theta_3), sin(theta_3)* sin(7 * theta_2), Y,
			-cos(theta_3) * sin(7 * theta_2), -sin(theta_3) * sin(7 * theta_2), cos(7 * theta_2), Z,
			0, 0, 0, 1;
		T_34 << -sin(theta_4), -cos(theta_4), 0, 0, 0, 0, -1, 0, cos(theta_4), -sin(theta_4), 0, L2, 0, 0, 0, 1;
		T_45 << cos(theta_5), -sin(theta_5), 0, 0, 0, 0, 1, 0, -sin(theta_5), -cos(theta_5), 0, 0, 0, 0, 0, 1;
		T_56 << cos(theta_6), -sin(theta_6), 0, 0, 0, 0, -1, 0, sin(theta_6), cos(theta_6), 0, 0, 0, 0, 0, 1;
		T_67 << 1, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, L3, 0, 0, 0, 1;
		T_07 = T_H0 *T_01 * T_12 * T_23 * T_34 * T_45 * T_56 * T_67;
		RotMatrix << T_07(0, 0), T_07(0, 1), T_07(0, 2), T_07(1, 0), T_07(1, 1), T_07(1, 2), T_07(2, 0), T_07(2, 1), T_07(2, 2);
		moveDirectionNew = RotMatrix * moveDirection;
		vector<double> positionPosture(9, 0);
		positionPosture[0] = T_07(0, 3);
		positionPosture[1] = T_07(1, 3);
		positionPosture[2] = T_07(2, 3);
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > -pi / 2 && atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < pi / 2)
		{
			positionPosture[3] = atan2(T_07(2, 1), T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(T_07(1, 0), T_07(0, 0));
		}
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < -pi / 2 || atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > pi / 2)
		{
			positionPosture[3] = atan2(-T_07(2, 1), -T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(-T_07(1, 0), -T_07(0, 0));
		}
		positionPosture[6] = moveDirectionNew(0);
		positionPosture[7] = moveDirectionNew(1);
		positionPosture[8] = moveDirectionNew(2);
		return positionPosture;
	
	}

	//向逆运动学写入参数
	void Param_writeto_Inv_sol(vector<double> &cartesian_in){
		theta_x = cartesian_in[3];
        theta_y = cartesian_in[4];
        theta_z = cartesian_in[5];
        Px = cartesian_in[0];
        Py = cartesian_in[1];
        Pz = cartesian_in[2];
	}

	//更新关节角
	void Param_joints_update(vector<double> motor_angles_in){ //m_dj_1- m_dj_4为关机转角，m2为柔性偏摆角，m3为柔性自转角

		//double Line_1, Line_2, Line_3, Line_4, Line;
		double m2_temp,m3_temp;

		Line_1 = Line + 4.5*motor_angles_in[4] / 6;//m_dj_1- m_dj_4/安装时注意电机顺序
		Line_2 = Line + 4.5*motor_angles_in[5] / 6;
		Line_3 = Line + 4.5*motor_angles_in[6] / 6;
		Line_4 = Line + 4.5*motor_angles_in[7] / 6;

		if (Line_1 < Line_3)
		{
			m3_temp = atan((Line_2*Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2*Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3));
		}
		else if (Line_1 > Line_3)
		{
			m3_temp = atan((Line_2*Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2*Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3)) + pi;
		}
		else
		{
			m3_temp = pi;
		}
		m2_temp = 2 * asin((Line_1 + Line_3) / 4 * sqrt((Line_2*Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3)*(Line_2*Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3))
			+ (Line_2*Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3)*(Line_2*Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3)) / ((Line_4 * Line_1 + Line_4 * Line_3 + Line_2 * Line_1 + Line_2 * Line_3) * R);
		
		theta_1 = motor_angles_in[0];
		theta_2=m3_temp;
		theta_3=m3_temp;
		theta_4 = motor_angles_in[1];
		theta_5 = motor_angles_in[2];
		theta_6 = motor_angles_in[3];
	}
    
	//更新关节角，仿真环境中使用
	void Param_joints_update_simulation(vector<double> joint_simulation_position_in)
	{
		theta_1 = joint_simulation_position_in[0];
		theta_2 = joint_simulation_position_in[1];
		theta_3 = joint_simulation_position_in[2];
		theta_4 = joint_simulation_position_in[3];
		theta_5 = joint_simulation_position_in[4];
		theta_6 = joint_simulation_position_in[5];
		// h = 238 - 52 / tan(pi / 2 - theta_1);
	}

	
};
